\chapter{Implementierung}
\section{Missionsplaner}

Der Missionsplaner wird, aufbauend auf der gegebenen Grundstruktur im Beispielprogramm, in gleicher Form erweitert.

Noch außerhalb der eigentlichen FSM werden in der Endlosschleife globale Variablen in Abhängigkeit der aktuellen Position berechnet, um sie dann in der FSM nutzen zu können. Dazu gehört auch die Berechnung, ob sich der Roboter auf dem Parcours befindet, welche die ihm nächste Linie ist und ob eine spezielle Kurve schon durchfahren wurde. Außerdem werden die Parklücken abgefragt und die Ausrichtung des Roboters berechnet, um dies nicht mehrfach innerhalb der FSM durchführen zu müssen.

Genaue Beschreibungen der einzelnen Schleifen und Befehle können aus den Kommentaren im Quellcode der GuidanceAT.java entnommen werden.

Die gegebene Main FSM wird insofern erweitert, dass innerhalb des 'While Action'-Abschnitts die jeweilige Sub FSM durch einen Switch- Befehl realisiert wird. Zur Speicherung des aktuellen und letzten Unterzustandes wird je eine Aufzählung implementiert, die an die der Main FSM angelehnt ist und somit 'currentScoutStatus' usw. heißt. Sie enthalten die in Kapitel \ref{sec_S} bis \ref{sec_LP} beschriebenen Unterzustände. Anhand der dort beschriebenen Zustandsübergangstabellen werden die Unterzustände grundlegend entwickelt. So werden z.B. beim ersten Eintritt in den Scout-Modus die Korrektur der Navigationsdaten und die Parklückensuche über
\begin{gather*}
navigation.setDetectionState(true);\\	 
navigation.setCorrectionState(true);
\end{gather*} 
aktiviert. Auf diese Zustandsübergänge wird nicht weiter eingegangen, weil sie über die Tabellen bereits beschrieben und so im Programm umgesetzt werden.

Um im Scout-Modus eine kurze Rückwärtsfahrt zu realisieren, wird die entsprechende Ausrichtung, der Abstand zur Kurve und die boolesche Variable, ob die Kurve bereits durchfahren wurde, überprüft. Dann kann kurzzeitig rückwärts gefahren werden.

Für Distanzen zu Punkten wird häufig die distanceTo-Funktion von getPose() verwendet. Zur Kalkulation werden Ziele in 'destination' und Ausgangspunkte als 'origin' zwischengespeichert.

Bei Beginn des Parkvorgangs wird anhand der am nächsten gelegenen Linie die Referenzausrichtung bestimmt, damit die Parkkoeffizienten berechnet und die Zielpose überprüft. Alle weiteren Programmschritte in Park This sind selbsterklärend.

In Park Now wird die am wenigsten entfernte Parklücke ermittelt, indem alle bekannten Parklücke durchlaufen werden, und wenn der Status 'GOOD' ist, mit in Betracht kommen. Wird eine neue Parklücke erkannt, ist die Distanz minimal, und es kann sofort eingeparkt werden. Um direkt in den Park This-Modus mit dem Untermodus Park zu gelangen, werden die entsprechenden Stati 'gefälscht', d.h. entsprechend der Erfordernisse gesetzt. So kann die Eingangsaktion von Park This übersprungen werden.

Am Ende des Parkvorgangs wird parkingFinished=true gesetzt. Im Modus Inactive macht dies den Unterschied zwischen einem normalen Pause-Modus und 'Pause in Parkzone' aus. Wird der Scout-Modus in der Parkzone gewählt, wird die FSM 'Leave Parkzone' aufgerufen. Ansonsten kann der Scout-Modus und der 'Park Now'-Modus nur aufgerufen werden, wenn sich der Roboter auf dem Parcours befindet.

Im 'Leave Parkzone'-Modus gibt es die Besonderheit, dass ein Liniensensorwert von weniger als 70 als schwarz bzw. als Linie interpretiert wird. So kann die Endposition schnell überprüft und korrigiert werden. Eine Zeitdifferenz kann effizient über eine zwischengespeicherte und die aktuelle Systemzeit berechnet und ausgewertet werden.

\section{Pfadgenerator}
Der Pfadgenerator bzw. die Koeffizientenberechnung wird komplett in eine Funktion 'ParkKoeff' ausgelagert und jeweils bei Erreichen der Einparkposition auf dem Parcours und bei Beginn des Ausparkvorgangs aufgerufen.

Der Funktion wird hierbei das Navigations-Element und eine boolesche Variable übergeben, die enthält, ob es sich um einen Ein- oder Ausparkvorgang handelt.
Je nach aktueller Parkzone, was anhand der Referenzausrichtung entschieden wird, wird die Zielposition (destination) berechnet. Sie befindet sich immer 45 cm hinter und 30 cm rechts vom Roboter für einen Einparkvorgang. Zum Ausparken werden die entgegengesetzten Richtungen genutzt. Die Ausgangsposition ist die aktuelle Position.

Wenn diese Punkte bestimmt wurden, kann daraus, auch wieder nach der booleschen Variable, berechnet werden, wo die Hilfspunkte B und C liegen müssen. Die Kalkulation der Koeffizienten orientiert sich dann an den Formeln \ref{koeff_P} bis \ref{koeff_S}.